#include <stdint.h>
#include <string.h>
#include <inttypes.h>
#include <stdbool.h>
 
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
 
#include "nvs.h"
#include "nvs_flash.h"
#include "esp_system.h"
#include "esp_log.h"
 
#include "host/ble_gap.h"
#include "host/ble_hs.h"
#include "host/util/util.h"
#include "console/console.h"
#include "services/gap/ble_svc_gap.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "modlog/modlog.h"
 
#define TAG "app"

/* 工作模式定义 */
#define MODE_SCANNER 0    // 扫描模式
#define MODE_ADVERTISER 1  // 广播模式
#define WORK_MODE MODE_ADVERTISER  // 当前工作模式

/* 设备名称定义 */
#define OWN_NAME (WORK_MODE == MODE_SCANNER ? "ESP32" : "SK-008")

#define BDASTR "%02X:%02X:%02X:%02X:%02X:%02X"
#define BDA2STR(x) (x)[0], (x)[1], (x)[2], (x)[3], (x)[4], (x)[5]
 
static char * addr_str(const void *addr);
static void print_uuid(const ble_uuid_t *uuid);
static void print_adv_fields(const struct ble_hs_adv_fields *fields);
static int blecent_gap_event(struct ble_gap_event *event, void *arg);
static void blecent_scan(void);
static void blecent_on_reset(int reason);
static void blecent_on_sync(void);
static void start_advertise(void);
 
 
static char * addr_str(const void *addr)
{
    static char buf[6 * 2 + 5 + 1];
    const uint8_t *u8p;
 
    u8p = addr;
    sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x",
            u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]);
 
    return buf;
}
 
static void print_uuid(const ble_uuid_t *uuid)
{
    char buf[BLE_UUID_STR_LEN];
    ESP_LOGI(TAG, "    %s", ble_uuid_to_str(uuid, buf));
}
 
static void print_adv_fields(const struct ble_hs_adv_fields *fields)
{
    if (fields->name == NULL){
        return;
    }
    
    const uint8_t *u8p;
    int i;
 
    if (fields->flags != 0) {
        ESP_LOGI(TAG, "- flags=0x%02X", fields->flags);
    }
 
    if (fields->uuids16 != NULL) {
        ESP_LOGI(TAG, "- uuids16(%scomplete)=", fields->uuids16_is_complete ? "" : "in");
        for (i = 0; i < fields->num_uuids16; i++) {
            print_uuid(&fields->uuids16[i].u);
        }
    }
 
    if (fields->uuids32 != NULL) {
        ESP_LOGI(TAG, "- uuids32(%scomplete)=", fields->uuids32_is_complete ? "" : "in");
        for (i = 0; i < fields->num_uuids32; i++) {
            print_uuid(&fields->uuids32[i].u);
        }
    }
 
    if (fields->uuids128 != NULL) {
        ESP_LOGI(TAG, "- uuids128(%scomplete)=", fields->uuids128_is_complete ? "" : "in");
        for (i = 0; i < fields->num_uuids128; i++) {
            print_uuid(&fields->uuids128[i].u);
        }
    }
 
    if (fields->name != NULL) {
        char *name = malloc(fields->name_len);
        memcpy(name, fields->name, fields->name_len);
        ESP_LOGI(TAG, "- name(%scomplete)=%s", fields->name_is_complete ? "" : "in", name);
        free(name);
    }
 
    if (fields->tx_pwr_lvl_is_present) {
        ESP_LOGI(TAG, "- tx_pwr_lvl=%d", fields->tx_pwr_lvl);
    }
 
    if (fields->slave_itvl_range != NULL) {
        ESP_LOGI(TAG, "- slave_itvl_range=");
        ESP_LOG_BUFFER_HEX(TAG, fields->slave_itvl_range, BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN);
    }
 
    if (fields->sm_tk_value_is_present) {
        ESP_LOGI(TAG, "- sm_tk_value=");
        ESP_LOG_BUFFER_HEX(TAG, fields->sm_tk_value, 16);
    }
 
    if (fields->sm_oob_flag_is_present) {
        ESP_LOGI(TAG, "- sm_oob_flag=%d", fields->sm_oob_flag);
    }
 
    if (fields->sol_uuids16 != NULL) {
        ESP_LOGI(TAG, "- sol_uuids16=");
        for (i = 0; i < fields->sol_num_uuids16; i++) {
            print_uuid(&fields->sol_uuids16[i].u);
        }
    }
 
    if (fields->sol_uuids32 != NULL) {
        ESP_LOGI(TAG, "- sol_uuids32=");
        for (i = 0; i < fields->sol_num_uuids32; i++) {
            print_uuid(&fields->sol_uuids32[i].u);
        }
    }
 
    if (fields->sol_uuids128 != NULL) {
        ESP_LOGI(TAG, "- sol_uuids128=");
        for (i = 0; i < fields->sol_num_uuids128; i++) {
            print_uuid(&fields->sol_uuids128[i].u);
        }
    }
 
    if (fields->svc_data_uuid16 != NULL) {
        ESP_LOGI(TAG, "- svc_data_uuid16=");
        ESP_LOG_BUFFER_HEX(TAG, fields->svc_data_uuid16, fields->svc_data_uuid16_len);
    }
 
    if (fields->public_tgt_addr != NULL) {
        u8p = fields->public_tgt_addr;
        for (i = 0; i < fields->num_public_tgt_addrs; i++) {
            ESP_LOGI(TAG, "- public_tgt_addr=%s", addr_str(u8p));
            u8p += BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN;
        }
    }
 
    if (fields->random_tgt_addr != NULL) {
        u8p = fields->random_tgt_addr;
        for (i = 0; i < fields->num_random_tgt_addrs; i++) {
            ESP_LOGI(TAG, "- random_tgt_addr=%s ", addr_str(u8p));
            u8p += BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN;
        }
    }
 
    if (fields->appearance_is_present) {
        ESP_LOGI(TAG, "- appearance=0x%04X", fields->appearance);
    }
 
    if (fields->adv_itvl_is_present) {
        ESP_LOGI(TAG, "- adv_itvl=0x%04X", fields->adv_itvl);
    }
 
    if (fields->device_addr_is_present) {
        u8p = fields->device_addr;
        ESP_LOGI(TAG, "- device_addr=%s", addr_str(u8p));
        u8p += BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN;
        ESP_LOGI(TAG, "- addr_type: %d ", *u8p);
    }
 
    if (fields->le_role_is_present) {
        ESP_LOGI(TAG, "- le_role=%d", fields->le_role);
    }
 
    if (fields->svc_data_uuid32 != NULL) {
        ESP_LOGI(TAG, "- svc_data_uuid32=");
        ESP_LOG_BUFFER_HEX(TAG, fields->svc_data_uuid32, fields->svc_data_uuid32_len);
    }
 
    if (fields->svc_data_uuid128 != NULL) {
        ESP_LOGI(TAG, "- svc_data_uuid128=");
        ESP_LOG_BUFFER_HEX(TAG, fields->svc_data_uuid128, fields->svc_data_uuid128_len);
    }
 
    if (fields->uri != NULL) {
        ESP_LOGI(TAG, "- uri=");
        ESP_LOG_BUFFER_HEX(TAG, fields->uri, fields->uri_len);
    }
 
    if (fields->mfg_data != NULL) {
        ESP_LOGI(TAG, "- mfg_data=");
        ESP_LOG_BUFFER_HEX(TAG, fields->mfg_data, fields->mfg_data_len);
    }
}
 
static int blecent_gap_event(struct ble_gap_event *event, void *arg)
{
    int rc = 0;
 
    switch (event->type) {
#if WORK_MODE == MODE_SCANNER
        /* 设备发现事件 */
        case BLE_GAP_EVENT_DISC:
        {
            // ESP_LOGI(TAG, "[" BDASTR "] type: %d, data_len: %d, rssi: %d", BDA2STR(event->disc.addr.val), event->disc.event_type, event->disc.length_data, event->disc.rssi);
 
            /* 解析 */
            struct ble_hs_adv_fields fields;
            rc = ble_hs_adv_parse_fields(&fields, event->disc.data, event->disc.length_data);
            if (rc != 0) {
                break;
            }
 
            /* 打印 */
            print_adv_fields(&fields);
 
            break;
        }
 
        /* 设备发现完成 */
        case BLE_GAP_EVENT_DISC_COMPLETE:
            MODLOG_DFLT(INFO, "discovery complete; reason=%d\n", event->disc_complete.reason);
            blecent_scan();
            break;
#else
        /* 广播模式下的事件处理 */
        case BLE_GAP_EVENT_ADV_COMPLETE:
            ESP_LOGI(TAG, "Advertisement complete, restarting...");
            start_advertise();
            break;
#endif
 
        default:
            break;
    }
 
    return rc;
}
 
static void blecent_scan(void)
{
    int rc;
 
    /* 获取地址类型 */
    uint8_t own_addr_type;
    rc = ble_hs_id_infer_auto(0, &own_addr_type);
    if (rc != 0) {
        MODLOG_DFLT(ERROR, "error determining address type; rc=%d\n", rc);
        return;
    }
 
    struct ble_gap_disc_params disc_params;
    disc_params.filter_duplicates = 1;
    disc_params.passive = 0;
    disc_params.itvl = 0;
    disc_params.window = 0;
    disc_params.filter_policy = 0;
    disc_params.limited = 0;
 
    rc = ble_gap_disc(own_addr_type, 5000, &disc_params, blecent_gap_event, NULL);
    if (rc != 0) {
        MODLOG_DFLT(ERROR, "Error initiating GAP discovery procedure; rc=%d\n", rc);
    }
}
 
static void blecent_on_reset(int reason)
{
    MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason);
}
 
static void start_advertise(void)
{
    int rc;
    struct ble_gap_adv_params adv_params;
    struct ble_hs_adv_fields fields;

    /* 设置广播参数 */
    memset(&adv_params, 0, sizeof(adv_params));
    adv_params.conn_mode = BLE_GAP_CONN_MODE_NON;
    adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN;
    adv_params.itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
    adv_params.itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;

    /* 设置广播数据 */
    memset(&fields, 0, sizeof(fields));
    fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
    fields.tx_pwr_lvl_is_present = 1;
    fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
    fields.name = (uint8_t *)OWN_NAME;
    fields.name_len = strlen(OWN_NAME);
    fields.name_is_complete = 1;

    rc = ble_gap_adv_set_fields(&fields);
    if (rc != 0) {
        ESP_LOGE(TAG, "Error setting advertisement data; rc=%d", rc);
        return;
    }

    /* 开始广播 */
    rc = ble_gap_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, BLE_HS_FOREVER,
                          &adv_params, NULL, NULL);
    if (rc != 0) {
        ESP_LOGE(TAG, "Error enabling advertisement; rc=%d", rc);
        return;
    }

    ESP_LOGI(TAG, "Advertisement started with name: %s", OWN_NAME);
}

static void blecent_on_sync(void)
{
    /* 配置地址 */
    if (ble_hs_util_ensure_addr(0) != 0) {
        return;
    }

#if WORK_MODE == MODE_SCANNER
    /* 启动扫描 */
    blecent_scan();
#else
    /* 启动广播 */
    start_advertise();
#endif
}
 
void blecent_host_task(void *param)
{
    nimble_port_run();
    nimble_port_freertos_deinit();
}
 
int app_main()
{
    /* 初始化NVS */
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ESP_ERROR_CHECK(nvs_flash_init());
    }
 
    /* 初始化控制器和NimBLE协议栈 */
    ret = nimble_port_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to init nimble %d ", ret);
        return -1;
    }
 
    /* 配置主机 */
    ble_hs_cfg.reset_cb = blecent_on_reset;
    ble_hs_cfg.sync_cb = blecent_on_sync;
    ble_hs_cfg.store_status_cb = ble_store_util_status_rr;
 
    /* 设置设备名 */
    if (ble_svc_gap_device_name_set(OWN_NAME) != 0) {
        ESP_LOGE(TAG, "set device name failed");
        return -1;
    }
 
    /* 使能NimBLE协议栈 */
    nimble_port_freertos_init(blecent_host_task);
 
    return 0;
}